/** @file m_intersection.cpp
	@brief Intersection between lines, planes and bounding shapes */

#include "math/m_intersection.h"

using namespace G3;

//==============================================================================
// Constructors and Destructors
//==============================================================================
G3::INTERSECTION::INTERSECTION () {
	Distance = 0.0;
	Flags = 0;
}

template <class T>
G3::INTERSECTION2<T>::INTERSECTION2 (): INTERSECTION () {
    // nop
}

template <class T>
G3::INTERSECTION3<T>::INTERSECTION3 (): INTERSECTION () {
    // nop
}

template <class T>
G3::RAY2<T>::RAY2 () {

}

template <class T>
G3::RAY2<T>::RAY2 (const VEC2<T> &Point0, const VEC2<T> &Point1) {
	a = Point0;
	b = Point1;
}

template <class T>
G3::RAY3<T>::RAY3 () {

}

template <class T>
G3::RAY3<T>::RAY3 (const VEC3<T> &Point0, const VEC3<T> &Point1) {
	a = Point0;
	b = Point1;
}

//==============================================================================
// = operators
//==============================================================================
template <class T>
INTERSECTION2<T>& G3::INTERSECTION2<T>::operator=(const INTERSECTION2<T> &I) {
	Distance = I.Distance;
	Point = I.Point;
	Flags = I.Flags;

	return *this;
}

template <class T>
INTERSECTION3<T>& G3::INTERSECTION3<T>::operator=(const INTERSECTION3<T> &I) {
	Distance = I.Distance;
	Point = I.Point;
	Flags = I.Flags;

	return *this;
}

template <class T>
POLYGON3<T>& G3::POLYGON3<T>::operator= (const POLYGON3<T> &p) {
	Vertices = p.Vertices;
	TexCoords = p.TexCoords;
	Normals = p.Normals;
	Plane = p.Plane;

	return *this;
}

//==============================================================================
// == operators
//==============================================================================
template <class T>
bool G3::POLYGON3<T>::operator== (const POLYGON3<T> &p) const {
	int i;

	if (Vertices.size () != p.Vertices.size ())
		return false;

	if (TexCoords.size () != p.TexCoords.size ())
		return false;

	if (Normals.size () != p.Normals.size ())
		return false;

	for (i=0; i<(int)Vertices.size (); i++)
		if (Vertices [i] != p.Vertices [i])
			return false;

	for (i=0; i<(int)TexCoords.size (); i++)
		if (TexCoords [i] != p.TexCoords [i])
			return false;

	for (i=0; i<(int)Normals.size (); i++)
		if (Normals [i] != p.Normals [i])
			return false;

	return true;
}

//==============================================================================
// Just returns the directional vector between a and b
//==============================================================================
template <class T>
VEC2<T> G3::RAY2<T>::GetVector () const {
	return (b - a);
}

template <class T>
VEC3<T> G3::RAY3<T>::GetVector () const {
	return (b - a);
}

//==============================================================================
// Calculates distance between the point and the ray
//==============================================================================
template <class T>
double G3::RAY3<T>::GetDistance (const VEC3<T> &p) const {
    VEC3<T> s = GetVector ();

    // a and b points are the same ?
    if (s.GetRootLength () < G3_EPSILON)
        return G3::GetDistance (a, p);

    s.Normalize ();

    VEC3<T> v = p - a;
    VEC3<T> d = a + s * (s * v);

    return G3::GetDistance (d, p);
}

//==============================================================================
// Just calculates the plane of the polygon
//==============================================================================
template <class T>
void G3::POLYGON3<T>::CalcPlane () {
	if (Vertices.size () > 2)
		Plane = PLANE3<T> (Vertices [0], Vertices [1], Vertices [2]);
}

template <class T>
void G3::POLYGON3<T>::Clear () {
	Vertices.clear ();
	TexCoords.clear ();
	Normals.clear ();
}

//==============================================================================
// Checks two 2D rays for intersection
//==============================================================================
template <class T>
INTERSECTION2<T> G3::RayRayIntersection (const RAY2<T> &a, const RAY2<T> &b) {
    VEC2<T> n = a.GetVector ().GetPerpendicular ();
    INTERSECTION2<T> I;
    T f = n * b.GetVector ();

    n.Normalize ();

    // Parallel?
    if (f == 0)
        return I;

    I.Distance = (n * (a.a - b.a)) / f;
    I.Point = b.a + (b.b - b.a) * I.Distance;
    G3_SETFLAG (I.Flags, I.INT_RAY);

    // Intersection within the line fragment ?
    if (I.Distance > -G3_EPSILON && I.Distance < 1.0 + G3_EPSILON)
        G3_SETFLAG (I.Flags, I.INT_LINE_FRAG);

    return I;
}

//==============================================================================
// Checks if a point is in a triangle
//
//	Thanks to Keidy from Mr-Gamemaker
//==============================================================================
template <class T>
bool G3::IsPointInTriangle (const VEC3<T> &A, const VEC3<T> &B, const VEC3<T> &C, const VEC3<T> &Point) {
	VEC3<T> e10 = B - A;
	VEC3<T> e20 = C - A;
	T a = e10 * e10;
	T b = e10 * e20;
	T c = e20 * e20;
	T ac_bb = (a * c) - (b * b);
	VEC3<T> vp = Point - A;
	T d = vp * e10;
	T e = vp * e20;
	T x = (d * c) - (e * b);
	T y = (e * a) - (d * b);
	T z = x + y - ac_bb;
	return (( ((uint &) z) & ~(((uint &) x) | ((uint &) y)) ) & 0x80000000);
}

//==============================================================================
// Calculates Ray - Plane intersection
//==============================================================================
template <class T>
INTERSECTION3<T> G3::RayPlaneIntersection (const PLANE3<T> &Plane, const RAY3<T> &Ray) {
	INTERSECTION3<T> I;

	I.Distance = (Plane.n * Ray.GetVector ());

	// Is the ray parallel to the plane?
	// I.Distance == 0
	if ((I.Distance > -G3_EPSILON) && (I.Distance < G3_EPSILON))
		return I;

	I.Distance = -(Plane.n * (Ray.a) + Plane.d) / I.Distance;
	//I.Distance = Plane.n * (Plane.p - Ray.a) / I.Distance;
	I.Point = Ray.a + Ray.GetVector () * I.Distance;
	G3_SETFLAG (I.Flags, I.INT_RAY);

	// Make sure the intersection is within the line fragment
	// 0 <= I.Distance <= 1
	if ((I.Distance > -G3_EPSILON) && (I.Distance < 1.0 + G3_EPSILON))
        G3_SETFLAG (I.Flags, I.INT_LINE_FRAG);

	return I;
}

//==============================================================================
// Calculates Ray - Triangle intersection
//==============================================================================
template <class T>
INTERSECTION3<T> G3::RayTriIntersection (const PLANE3<T> &Plane, const VEC3<T> &A, const VEC3<T> &B, const VEC3<T> &C, const RAY3<T> &Ray) {
	INTERSECTION3<T> I = RayPlaneIntersection (Plane, Ray);

    if (G3_TESTFLAG (I.Flags, I.INT_LINE_FRAG) && IsPointInTriangle (A, B, C, I.Point))
        G3_SETFLAG (I.Flags, I.INT_POLY);

	return I;
}

//==============================================================================
// Finds an intersection point between 3 planes
//==============================================================================
template <class T>
INTERSECTION3<T> G3::Intersect3Planes (const PLANE3<T> &Plane1, const PLANE3<T> &Plane2, const PLANE3<T> &Plane3) {
	INTERSECTION3<T> I;
	VEC3<T> n23 = CrossProduct (Plane2.n, Plane3.n);
	T denom = Plane1.n * n23;

    // denom == 0
	if (denom > -G3_EPSILON && denom < G3_EPSILON)
		return I;

    //! \bug Check the validity of this formula
	I.Point = (n23 * Plane1.d + CrossProduct (Plane3.n, Plane1.n) * Plane2.d + CrossProduct (Plane1.n, Plane2.n) * Plane3.d) / denom;

	G3_SETFLAG (I.Flags, I.INT_PLANES);

	return I;
}

//==============================================================================
// A simple Sutherland Hodgeman-style polygon splitting
//! \note Erases Front and / or Back polygons when their minimal width is below epsilon \see G3_EPSILON \see G3::MinDistRegression
//==============================================================================
template <class T>
void G3::SplitPolygonByPlane (const POLYGON3<T> &Poly, const PLANE3<T> &Plane, POLYGON3<T> *Front, POLYGON3<T> *Back) {
	Assert (Front, "SplitPolygonByPlane: Front polygon argument is a NULL pointer");
	Assert (Back, "SplitPolygonByPlane: Back polygon argument is a NULL pointer");

	VEC2<T> uv;
	VEC3<T> n;
	INTERSECTION3<T> I;
	T d1 = 0, d2 = 0;
//	HALFSPACE hs = ON_PLANE;
	bool back = false;
	int i = 0;

	Assert (!Poly.Vertices.empty (), "SplitPolygonByPlane: Attempting to split an invalid polygon");

	if (!Front->Vertices.empty ())
		Front->Vertices.clear ();
	if (!Front->TexCoords.empty ())
		Front->TexCoords.clear ();
	if (!Front->Normals.empty ())
		Front->Normals.clear ();
	if (!Back->Vertices.empty ())
		Back->Vertices.clear ();
	if (!Back->TexCoords.empty ())
		Back->TexCoords.clear ();
	if (!Back->Normals.empty ())
		Back->Normals.clear ();

/*    //! \todo Could be optimized / might even be a bug
    // Is the polygon's first vertex behind the plane?
	for (i=0; i<(int) Poly.Vertices.size (); i++) {
		hs = Plane.ClassifyPoint (Poly.Vertices [i]);
		if (hs != ON_PLANE) {
			back = (hs == NEGATIVE);
			break;
		}
	}*/

    // Is the polygon's first vertex behind the plane?
	back = (Plane.ClassifyPoint (Poly.Vertices [0]) == NEGATIVE);

    for (i=0; i<(int) Poly.Vertices.size (); i++) {
		if (!back) {
			Front->Vertices.push_back (Poly.Vertices [i]);
			if ((int) Poly.TexCoords.size () > i)
				Front->TexCoords.push_back (Poly.TexCoords [i]);
			if ((int) Poly.Normals.size () > i)
				Front->Normals.push_back (Poly.Normals [i]);
		} else {
			Back->Vertices.push_back (Poly.Vertices [i]);
			if ((int) Poly.TexCoords.size () > i)
				Back->TexCoords.push_back (Poly.TexCoords [i]);
			if ((int) Poly.Normals.size () > i)
				Back->Normals.push_back (Poly.Normals [i]);
		}

       	// Find Polygon Edge and SlicingPlane intersection points
       	I = RayPlaneIntersection (Plane, RAY3<T> (Poly.Vertices [i], Poly.Vertices [(i + 1) % ((int) Poly.Vertices.size ())]));

        if (G3_TESTFLAG (I.Flags, I.INT_LINE_FRAG)) {
            // The distance between the corner vertices
            d1 = (Poly.Vertices [(i + 1) % ((int) Poly.Vertices.size ())] - Poly.Vertices [i]).GetLength ();
            // The distance from the last vertex to this intersection point
            d2 = (I.Point - Poly.Vertices [i]).GetLength ();

            // Interpolate the new texture coordinates
			if (Poly.TexCoords.size () == Poly.Vertices.size ()) {
				uv = Poly.TexCoords [(i + 1) % ((int) Poly.TexCoords.size ())] - Poly.TexCoords [i];

				if (d1 != 0)
					uv = Poly.TexCoords [i] + uv * (d2 / d1);
				else
					uv = Poly.TexCoords [i] + uv * d2;

				Front->TexCoords.push_back (uv);
				Back->TexCoords.push_back (uv);
			}
			// Interpolate the new normal
			if (Poly.Normals.size () == Poly.Vertices.size ()) {
				n = Poly.Normals [(i + 1) % ((int) Poly.Normals.size ())] * d1 + Poly.Normals [i] * d2;

				if (G3_ABS (n.x) + G3_ABS (n.y) + G3_ABS (n.z) > G3_EPSILON)
                    n.Normalize ();

				Front->Normals.push_back (n);
				Back->Normals.push_back (n);
			}
	        // Push the intersection points into both polygons
            Front->Vertices.push_back (I.Point);
            Back->Vertices.push_back (I.Point);
            // Swap the current side
            back = !back;
        } else {
            //! \todo Suspicious..
            //! \todo Optimize..
			// One of the ray's points is on the plane?
			if (I.Distance > 1.0 - G3_EPSILON && I.Distance < 1.0 + G3_EPSILON) {
		        // Push the vertex into the other face aswell
				if (back)
		            Front->Vertices.push_back (Poly.Vertices [i]);
				else
		            Back->Vertices.push_back (Poly.Vertices [i]);

		        // Push the texture coordinates also into the other face
				if (Poly.TexCoords.size () == Poly.Vertices.size ()) {
					if (back)
						Front->TexCoords.push_back (Poly.TexCoords [i]);
					else
						Back->TexCoords.push_back (Poly.TexCoords [i]);
				}
		        // Push the normal also into the other face
				if (Poly.Normals.size () == Poly.Vertices.size ()) {
					if (back)
						Front->Normals.push_back (Poly.Normals [i]);
					else
						Back->Normals.push_back (Poly.Normals [i]);
				}
			}
		}
    }

    // Erase degenerate triangles
    if (MinDistRegression (*Front) < G3_EPSILON)
        Front->Clear ();
    if (MinDistRegression (*Back) < G3_EPSILON)
        Back->Clear ();
}

//==============================================================================
// Classifies a polygon in respect to a plane
//==============================================================================
template <class T>
HALFSPACE G3::PlaneClassifyPolygon (const PLANE3<T> &Plane, const POLYGON3<T> &Poly) {
	int i, front = 0, back = 0, onplane = 0;
	double res = 0.0;

	for (i=0; i<(int) Poly.Vertices.size (); i++) {
		res = Plane.GetDistance (Poly.Vertices [i]);

		if (res > G3_EPSILON)
			front++;
		else if (res < -G3_EPSILON)
			back++;
		else
			onplane++;
	}

	if (front == (int) Poly.Vertices.size ())
		return POSITIVE;
	if (back == (int) Poly.Vertices.size ())
		return NEGATIVE;
	if (onplane == (int) Poly.Vertices.size ())
		return ON_PLANE;
	return SPANNING;
}

//==============================================================================
// Classifies a triangle in respect to a plane
//==============================================================================
template <class T>
HALFSPACE G3::PlaneClassifyTriangle (const PLANE3<T> &Plane, const VEC3<T> &A, const VEC3<T> &B, const VEC3<T> &C) {
	int front = 0, back = 0, onplane = 0;
	double res = 0.0;

	res = Plane.GetDistance (A);
	if (res > G3_EPSILON)
		front++;
	else if (res < -G3_EPSILON)
		back++;
	else
		onplane++;

	res = Plane.GetDistance (B);
	if (res > G3_EPSILON)
		front++;
	else if (res < -G3_EPSILON)
		back++;
	else
		onplane++;

	res = Plane.GetDistance (C);
	if (res > G3_EPSILON)
		front++;
	else if (res < -G3_EPSILON)
		back++;
	else
		onplane++;

	if (front == 3)
		return POSITIVE;
	if (back == 3)
		return NEGATIVE;
	if (onplane == 3)
		return ON_PLANE;
	return SPANNING;
}

//==============================================================================
// Triangulates a regular polygon
//==============================================================================
template <class T>
std::vector<POLYGON3<T> > G3::TriangulateRegPoly (const POLYGON3<T> &Polygon) {
	std::vector<POLYGON3<T> > TriList;
	POLYGON3<T> Result;
	int n = ((int) Polygon.Vertices.size ()) - 1;
	int i = 0;

	Assert (Polygon.Vertices.size () > 3, "TriangulateRegPoly: Nothing to triangulate");

	while (i < n - 1) {
		Result.Clear ();
		Result.Vertices.push_back (Polygon.Vertices [i]);
		Result.Vertices.push_back (Polygon.Vertices [i + 1]);
		Result.Vertices.push_back (Polygon.Vertices [n]);

		if (Polygon.TexCoords.size () == Polygon.Vertices.size ()) {
            Result.TexCoords.push_back (Polygon.TexCoords [i]);
            Result.TexCoords.push_back (Polygon.TexCoords [i + 1]);
            Result.TexCoords.push_back (Polygon.TexCoords [n]);
		}

		if (Polygon.Normals.size () == Polygon.Vertices.size ()) {
            Result.Normals.push_back (Polygon.Normals [i]);
            Result.Normals.push_back (Polygon.Normals [i + 1]);
            Result.Normals.push_back (Polygon.Normals [n]);
		}

		Result.CalcPlane ();
		TriList.push_back (Result);

		++i;
	}

	return TriList;
}

//==============================================================================
// Returns minimal vertex distance to the regression line of a polygon
//==============================================================================
template <class T>
double G3::MinDistRegression (const POLYGON3<T> &Polygon) {
    double dist = 0.0, min_dist = G3_MAX_DOUBLE;
    RAY3<T> line;
    int i, j, k;

    for (i=0; i<(int)Polygon.Vertices.size (); i++) {
        for (j=i; j<(int)Polygon.Vertices.size (); j++) {
            line.a = Polygon.Vertices [i];
            line.b = Polygon.Vertices [j];

            for (k=0; k<(int)Polygon.Vertices.size (); k++) {
                if (k != i && k != j) {
                    dist = line.GetDistance (Polygon.Vertices [k]);

                    if (dist < G3_EPSILON)
                        return dist;

                    if (dist < min_dist)
                        min_dist = dist;
                }
            }
        }
    }

    return min_dist;
}

//==============================================================================
// Tests if a point is inside a triangle
//==============================================================================
/*
template <class T>
bool G3::TestPointTriangle (const VEC3<T> &P, const VEC3<T> &A, const VEC3<T> &B, const VEC3<T> &C) {
    VEC3<T> v = Barycentric (A, B, C, P);

    return (v.y >= 0 && v.z >= 0 && (v.y + v.z) <= 1);
}*/

//==============================================================================
// Explicit template instantiation
//==============================================================================
template class G3::INTERSECTION2<float>;
template class G3::INTERSECTION2<double>;
template class G3::INTERSECTION3<float>;
template class G3::INTERSECTION3<double>;

template class G3::RAY2<float>;
template class G3::RAY2<double>;
template class G3::RAY3<float>;
template class G3::RAY3<double>;

template class G3::POLYGON3<float>;
template class G3::POLYGON3<double>;

template INTERSECTION2<float> G3::RayRayIntersection (const RAY2<float> &a, const RAY2<float> &b);
template INTERSECTION2<double> G3::RayRayIntersection (const RAY2<double> &a, const RAY2<double> &b);

template bool G3::IsPointInTriangle (const VEC3<float> &A, const VEC3<float> &B, const VEC3<float> &C, const VEC3<float> &Point);
template bool G3::IsPointInTriangle (const VEC3<double> &A, const VEC3<double> &B, const VEC3<double> &C, const VEC3<double> &Point);

template INTERSECTION3<float> G3::RayPlaneIntersection (const PLANE3<float> &Plane, const RAY3<float> &Ray);
template INTERSECTION3<double> G3::RayPlaneIntersection (const PLANE3<double> &Plane, const RAY3<double> &Ray);

template INTERSECTION3<float> G3::RayTriIntersection (const PLANE3<float> &Plane, const VEC3<float> &A, const VEC3<float> &B, const VEC3<float> &C, const RAY3<float> &Ray);
template INTERSECTION3<double> G3::RayTriIntersection (const PLANE3<double> &Plane, const VEC3<double> &A, const VEC3<double> &B, const VEC3<double> &C, const RAY3<double> &Ray);

template INTERSECTION3<float> G3::Intersect3Planes (const PLANE3<float> &Plane1, const PLANE3<float> &Plane2, const PLANE3<float> &Plane3);
template INTERSECTION3<double> G3::Intersect3Planes (const PLANE3<double> &Plane1, const PLANE3<double> &Plane2, const PLANE3<double> &Plane3);

template void G3::SplitPolygonByPlane (const POLYGON3<float> &Poly, const PLANE3<float> &Plane, POLYGON3<float> *Front, POLYGON3<float> *Back);
template void G3::SplitPolygonByPlane (const POLYGON3<double> &Poly, const PLANE3<double> &Plane, POLYGON3<double> *Front, POLYGON3<double> *Back);

template HALFSPACE G3::PlaneClassifyPolygon (const PLANE3<float> &Plane, const POLYGON3<float> &Poly);
template HALFSPACE G3::PlaneClassifyPolygon (const PLANE3<double> &Plane, const POLYGON3<double> &Poly);

template HALFSPACE G3::PlaneClassifyTriangle (const PLANE3<float> &Plane, const VEC3<float> &A, const VEC3<float> &B, const VEC3<float> &C);
template HALFSPACE G3::PlaneClassifyTriangle (const PLANE3<double> &Plane, const VEC3<double> &A, const VEC3<double> &B, const VEC3<double> &C);

template std::vector<POLYGON3<float> > G3::TriangulateRegPoly (const POLYGON3<float> &Polygon);
template std::vector<POLYGON3<double> > G3::TriangulateRegPoly (const POLYGON3<double> &Polygon);

template double G3::MinDistRegression (const POLYGON3<float> &Polygon);
template double G3::MinDistRegression (const POLYGON3<double> &Polygon);

/*template bool G3::TestPointTriangle (const VEC3<int> &P, const VEC3<int> &A, const VEC3<int> &B, const VEC3<int> &C);
template bool G3::TestPointTriangle (const VEC3<float> &P, const VEC3<float> &A, const VEC3<float> &B, const VEC3<float> &C);
template bool G3::TestPointTriangle (const VEC3<double> &P, const VEC3<double> &A, const VEC3<double> &B, const VEC3<double> &C);*/
